#include "BPhysicsActor.h"
#include "BPhysicsMotionState.h"
#include "DirectXMesh.h"
#include "BPhysicsEngine.h"
#include "Engine.h"
#include "GameState.h"
#include "BulletCollision/Gimpact/btGImpactShape.h"
#include "BehaviourI.h"
#include "BehaviourInfo.h"
#include "BPhysicsRayCallBack.h"
#include "AI_Perceiver.h"
#include "ResourceManager.h"
#include <sstream>
#include <BulletCollision/CollisionShapes/btConvexHullShape.h>
#include "Grid.h"


/* Audio hack: quick test */
void BPhysicsActor::CreateEngineAudio()
{
    //Create a WAV
    Audio_Engine::audio_id wavID = 
    Audio_Engine::AudioManager::Instance()->CreateWAVNode("bikeEngine.wav", true, 0.1f, 0.1f);
    Audio_Engine::Safe_Caster<Audio_Engine::Wav_Player> scWAV;
    //Store it here.
    m_wavPlayerNode = scWAV.SafeCast( Audio_Engine::AudioManager::Instance()->GetNode(wavID));

    //Create a position Node
    Audio_Engine::audio_id posID = 
    Audio_Engine::AudioManager::Instance()->CreatePositionNode(wavID, 100.0f, true, 0,0,0);
    Audio_Engine::Safe_Caster<Audio_Engine::Position_Node> scPos;
    //Store it ehre
    m_engineNode = scPos.SafeCast( Audio_Engine::AudioManager::Instance()->GetNode(posID));
}

/************************************************************************/
/* MESSAGING                                                            */
/************************************************************************/
void BPhysicsActor::vProcessInbox()
{
    for(unsigned int i = 0 ; i < m_inBox.size(); ++i)
    {
        switch(m_inBox[i].m_type)
        {
        case MessageSystem::GO_HERE:
            {
                m_info.SetTargetPathNode(*(int*)m_inBox[i].m_data);
                m_manager.ChangeState("PathState");
                break;
            }
        case MessageSystem::TARGET_THIS : 
            {
                BPhysicsActor* act = static_cast<BPhysicsActor*>(m_inBox[i].m_data);
                //Error checking!
                if(!act)
                    assert(0);
                SetTarget(act);
                m_manager.ChangeState("SeekState");
                break;
            }
        case MessageSystem::RELOAD_TEXTURES:
            {
                if(m_mesh)
                    m_mesh->ReFetchTextures();
                break;
            }
        }
    }
    m_inBox.clear();
}

/************************************************************************/
/*------------------------------LUA-------------------------------------*/
/************************************************************************/
void BPhysicsActor::LSetPosition( float x, float y, float z )
{
    SetPosition(btVector3(x,y,z));
}
LuaPlus::LuaObject BPhysicsActor::GetMetaTable()
{
    return *Static_MetaTable;
}
void BPhysicsActor::InitializeStateManager(const char* luaObj)
{
    m_manager.SetUp(this, GetMetaTable(), luaObj);
}
LuaPlus::LuaObject* BPhysicsActor::Static_MetaTable = 0;
LuaPlus::LuaObject* BPhysicsActor::StaticInitializeLuaMetaTable()
{
    Static_MetaTable = new LuaPlus::LuaObject;
    *Static_MetaTable = LuaManager::Instance()->GetOwner()->GetGlobals().CreateTable("BPhysicsActorTable");
    Static_MetaTable->SetObject("__index", *Static_MetaTable);
    LUA_REGISTER("LSetPosition",BPhysicsActor, &BPhysicsActor::LSetPosition);
    LUA_REGISTER("LApplyForce",BPhysicsActor, &BPhysicsActor::LApplyForce);
    LUA_REGISTER("LChangeState",BPhysicsActor, &BPhysicsActor::LChangeState);
    LUA_REGISTER("LAdjustPosition",BPhysicsActor, &BPhysicsActor::LAdjustPosition);
    LUA_REGISTER("LSetVelocity",BPhysicsActor, &BPhysicsActor::LSetVelocity);
    LUA_REGISTER("LSetBehaviour",BPhysicsActor, &BPhysicsActor::LSetBehaviour);
    LUA_REGISTER("LSetUpBehaviourInfo",BPhysicsActor, &BPhysicsActor::LSetUpBehaviourInfo);
    LUA_REGISTER("LGetNearestPathNode",BPhysicsActor, &BPhysicsActor::LGetNearestPathNode);
    LUA_REGISTER("LUpdateBehaviour",BPhysicsActor, &BPhysicsActor::LUpdateBehaviour);
    LUA_REGISTER("LPostMessage",BPhysicsActor, &BPhysicsActor::LPostMessage);
    LUA_REGISTER("LSetUpPerceptionPerspective",BPhysicsActor, &BPhysicsActor::LSetUpPerceptionPerspective);
    LUA_REGISTER("LSetUpPerceptionOrthographic",BPhysicsActor, &BPhysicsActor::LSetUpPerceptionOrthographic);
    return Static_MetaTable;
}
void BPhysicsActor::LApplyForce( float x, float y, float z )
{
    ApplyCentralForce(btVector3(x,y,z));
}
void BPhysicsActor::LChangeState( LuaPlus::LuaObject newState )
{
    m_manager.ChangeState(newState);
}
void BPhysicsActor::LAdjustPosition( float x, float y, float z )
{
    SetPosition(GetPosition() + btVector3(x,y,z));
}
void BPhysicsActor::LUpdateBehaviour()
{
    if(m_behaviour)
    {
        float arr[3] = {0,0,0};
        m_behaviour->CalcMotion(arr, &m_info, 1.0f);
        ApplyCentralForce(btVector3(arr[0],arr[1],arr[2]));
    }
}
void BPhysicsActor::LSetBehaviour(int name)
{
    //m_behaviour = Engine::Instance()->GetBehaviour(name);
}
void BPhysicsActor::LSetUpBehaviourInfo(float maxVelocity, int targetNode, float radius)
{
    m_info.SetSourceActor(this);
    if(m_target)
        m_info.SetTargetActor(m_target);

    m_info.SetMaxVelocity(maxVelocity);
    m_info.SetTargetPathNode(targetNode);
    m_info.SetGrid(m_grid);
    m_AiCollisionRadius = radius;
}
int BPhysicsActor::LGetNearestPathNode()
{
    btTransform t;
    m_motionState->getWorldTransform(t);
    return m_grid->GetNearestNode(D3DXVECTOR3(t.getOrigin().x(),
        t.getOrigin().y(),
        t.getOrigin().z()), m_AiCollisionRadius);
}
void BPhysicsActor::LSetVelocity( float x, float y, float z )
{
    if(m_body)
        m_body->setLinearVelocity(btVector3(x,y,z));
}
void BPhysicsActor::LPostMessage(int which, float time)
{
    switch(which)
    {
    case MessageSystem::GO_HERE:
        {
            MessageSystem::Ai_Message msg(MessageSystem::GO_HERE,time);
            msg.m_data = &m_info.GetTargetPathNode();
            m_outBox.push_back(msg);
            break;
        }
    case MessageSystem::TARGET_THIS : 
        {
            MessageSystem::Ai_Message msg(MessageSystem::TARGET_THIS,time);
            msg.m_data = m_target;
            m_outBox.push_back(msg);
            break;
        }
    }
}
void BPhysicsActor::LSetUpPerceptionOrthographic( int resX, int resY,int width, int height, float n, float f )
{
    SetUpPerception(resX,resY);
    CreateOrthoProjection(width, height, n,f);
}
void BPhysicsActor::LSetUpPerceptionPerspective( int resX, int resY,float fOv, float aspect, float n, float f )
{
    SetUpPerception(resX,resY);
    CreatePerspProjection(fOv,aspect,n,f);
}
/* Construction */
BPhysicsActor::BPhysicsActor(float mass, const btVector3& scale, Grid* g) : 
m_addedToMap(0),
    m_resX(0),
    m_resY(0),
    m_projectionType(-1),
    m_averageColor(0.0f),
    m_mesh(NULL),
    m_body(NULL),
    m_shape(NULL),
    m_motionState(NULL),
    m_meshIndexs(NULL),
    m_meshVertexs(NULL),
    m_array(NULL),
    m_triangleMesh(NULL),
    m_scale(scale),
    m_mass(mass),
    m_thinking(false),
    m_target(NULL),
    m_maxVelocity(0.0f),
    m_isStaticObstacle(false),
    m_AiCollisionRadius(-1.0f),
    m_vehicle(NULL),
    m_rayCaster(NULL),
    m_tyreMesh(NULL),
    m_steeringValue(0.0f),
    m_breakingForce(0.0f),
    m_maxBrakes(50.0f),
    m_steerResetSpeed(0.07f),
    m_grid(g),
    m_isAwake(true),
    m_aip_Obj(0),
    m_perceiver(0),
    m_type(BPhysicsActor::Actor_Boring),
    m_behaviour(0),
    m_heading(0,0,1),
    m_engineNode(NULL),
    m_wavPlayerNode(NULL)
{
    D3DXMatrixIdentity(&m_d3dTransform);
    m_transform.setIdentity();
    for(int i = 0; i < BPhysicsActor::NUM_CONTROLLS; ++i)
        m_controlls[i] = false;
}
/************************************************************************/
/* CREATION                                                             */
/************************************************************************/
void BPhysicsActor::CreateConvexHull(D3DXMATRIXA16& initialTransform)
{
    //m_type = BPhysicsActor::Actor_Dynamic;
    ////copy the vert indexs into a linear array
    //m_meshIndexs = new int[m_mesh->Triangles().size()*3];
    //for(unsigned int i = 0; i < m_mesh->Triangles().size(); ++ i)
    //{
    //    m_meshIndexs[(i*3)] =       (int)m_mesh->Triangles()[i].m_vertIndex[0];
    //    m_meshIndexs[(i*3) + 1] =   (int)m_mesh->Triangles()[i].m_vertIndex[1];
    //    m_meshIndexs[(i*3) + 2] =   (int)m_mesh->Triangles()[i].m_vertIndex[2];
    //}
    ////copy the vertex data over
    //m_meshVertexs = new btScalar[m_mesh->Triangles().size()*3* 3];
    //for(unsigned int i = 0; i < m_mesh->Verts().size(); ++ i)
    //{
    //    D3DXVECTOR3 vert = m_mesh->Vert(i);
    //    D3DXVECTOR4 transformed;
    //    D3DXVec3Transform(&transformed, &vert, &initialTransform);
    //    D3DXVECTOR3 converted(transformed);
    //    m_meshVertexs[(i*3)]        = converted.x;
    //    m_meshVertexs[(i*3) + 1]    = converted.y;
    //    m_meshVertexs[(i*3) + 2]    = converted.z;
    //}
    //m_array = new btTriangleIndexVertexArray(   m_mesh->Triangles().size(),
    //    m_meshIndexs,
    //    12,
    //    m_mesh->Triangles().size()*3*3,
    //    m_meshVertexs,
    //    12);
    //m_shape = new btCompoundShape();
    //btTransform t; t.setIdentity();
    //btConvexTriangleMeshShape* ptr = new btConvexTriangleMeshShape(m_array);
    //m_shape->addChildShape(t,ptr);
    //m_shape->setLocalScaling(m_scale);


    /*m_type = BPhysicsActor::Actor_Dynamic;
    m_triangleMesh = new btTriangleMesh();
    for(vTriangle::const_iterator it = m_mesh->Triangles().begin();
    it != m_mesh->Triangles().end();
    ++it)
    {
    D3DXVECTOR3 arr[3];
    arr[0]   = m_mesh->Vert(it->m_vertIndex[0]);
    arr[1]   = m_mesh->Vert(it->m_vertIndex[1]);
    arr[2]   = m_mesh->Vert(it->m_vertIndex[2]);
    m_triangleMesh->addTriangle(btVector3(arr[0].x, arr[0].y, arr[0].z),
    btVector3(arr[1].x, arr[1].y, arr[1].z),
    btVector3(arr[2].x, arr[2].y, arr[2].z));
    }
    m_shape = new btCompoundShape();
    btConvexTriangleMeshShape* shape = new btConvexTriangleMeshShape(m_triangleMesh);
    btTransform t; t.setIdentity();
    m_shape->addChildShape(t, shape);
    m_shape->setLocalScaling(m_scale);*/

    m_type = BPhysicsActor::Actor_Dynamic;
    m_meshVertexs = new float[m_mesh->Triangles().size()*3*3];
    int numDone = 0;
    for(vTriangle::const_iterator it = m_mesh->Triangles().begin();
        it != m_mesh->Triangles().end();
        ++it)
    {
        D3DXVECTOR3 arr[3];
        arr[0]   = m_mesh->Vert(it->m_vertIndex[0]);
        arr[1]   = m_mesh->Vert(it->m_vertIndex[1]);
        arr[2]   = m_mesh->Vert(it->m_vertIndex[2]);
        D3DXVECTOR4 transformed[3];
        for(int i = 0; i < 3; ++i)
            D3DXVec3Transform(&transformed[i], &arr[i], &initialTransform);
        D3DXVECTOR3 converted[3] = {transformed[0],transformed[1],transformed[2]};
        memcpy(m_meshVertexs+numDone, converted, sizeof(float)*9);
        numDone += 9;
        //shape->addPoint(btVector3(converted[0].x, converted[0].y, converted[0].z));
        //shape->addPoint(btVector3(converted[1].x, converted[1].y, converted[1].z));
        //shape->addPoint(btVector3(converted[2].x, converted[2].y, converted[2].z));
    }
    btConvexHullShape* shape = new btConvexHullShape(m_meshVertexs,m_mesh->Triangles().size()*3,12);
    m_shape = new btCompoundShape();
    btTransform t; t.setIdentity();
    m_shape->addChildShape(t, shape);
    m_shape->setLocalScaling(m_scale);
} 
void BPhysicsActor::CreateBVH(D3DXMATRIXA16& initialTransform)
{
    m_type = BPhysicsActor::Actor_Boring;
    m_triangleMesh = new btTriangleMesh();
    for(vTriangle::const_iterator it = m_mesh->Triangles().begin();
        it != m_mesh->Triangles().end();
        ++it)
    {
        D3DXVECTOR3 arr[3];
        arr[0]   = m_mesh->Vert(it->m_vertIndex[0]);
        arr[1]   = m_mesh->Vert(it->m_vertIndex[1]);
        arr[2]   = m_mesh->Vert(it->m_vertIndex[2]);
        D3DXVECTOR4 transformed[3];
        for(int i = 0; i < 3; ++i)
            D3DXVec3Transform(&transformed[i], &arr[i], &initialTransform);
        D3DXVECTOR3 converted[3] = {transformed[0],transformed[1],transformed[2]};
        m_triangleMesh->addTriangle(
            btVector3(converted[0].x, converted[0].y, converted[0].z),
            btVector3(converted[1].x, converted[1].y, converted[1].z),
            btVector3(converted[2].x, converted[2].y, converted[2].z));
    }
    btBvhTriangleMeshShape* shape = new btBvhTriangleMeshShape(m_triangleMesh,true,true);
    shape->setMargin(0.2f);
    m_shape = new btCompoundShape();
    btTransform t; t.setIdentity();
    m_shape->addChildShape(t, shape);
    m_shape->recalculateLocalAabb();
    m_shape->setLocalScaling(m_scale);
}
void BPhysicsActor::GenerateBody()
{
    //create the rigid body member
    if(!m_motionState && !m_body)
    {
        btTransform t;
        t.setIdentity();
        m_motionState = new BPhysicsMotionState(t, m_scale);
        btVector3 inertia(0,0,0);

        if(!m_shape->isNonMoving())
            m_shape->calculateLocalInertia(m_mass,inertia);
        btRigidBody::btRigidBodyConstructionInfo ri(m_mass,m_motionState,m_shape,inertia);
        m_body = new btRigidBody(ri);
        m_body->setActivationState(ACTIVE_TAG);
    }
}
bool BPhysicsActor::CreateAsVehicle( const char* name)
{
    m_type = BPhysicsActor::Actor_Dynamic;
    m_mesh = GET_MESH("car_physics");

    //make the tyres
    m_tyreMesh = GET_MESH("tyreMesh");
    m_tyreMesh->SetTexture(GET_TEXTURE("tyreTexture.png"));
    m_tyreMesh->ReFetchTextures();
    //m_tyreMesh->SetTexture(GET_TEXTURE("tyreTexture.png"));
    m_transform.setRotation(btQuaternion(btVector3(0,1,0), -90*(float)M_PI/180.0f));
    float transArray[16];
    m_transform.getOpenGLMatrix(transArray);
    D3DXMATRIXA16 transform(transArray);
    CreateConvexHull(transform);
    m_mesh = GET_MESH(name);
    GenerateBody();
    /************************************************************************/
    /* CREATE THE VEHICLE                                                   */
    /************************************************************************/
    m_rayCaster = new btDefaultVehicleRaycaster(Engine::Instance()->GetCurrentState()->GetPhysicsEngine()->GetWorld());
    if (!m_rayCaster)  
        return false; //LEAKS IF FALSE
    btRaycastVehicle::btVehicleTuning tuning;
    tuning.m_suspensionStiffness = 40.0f;
    tuning.m_suspensionCompression = 2.4f;
    tuning.m_suspensionDamping = 2.3f;
    tuning.m_maxSuspensionTravelCm = 200;
    tuning.m_frictionSlip = 110.1f;
    tuning.m_maxSuspensionForce = 4000.0f;

    m_vehicle = new btRaycastVehicle(tuning,m_body,m_rayCaster);
    if (!m_vehicle)      
        return false; //LEAKS IF FALSE
    m_vehicle->setCoordinateSystem(0,1,2);
    btVector3 axle(-1,0,0);
    btVector3 axis(0,-1,0);
    /* Front Right */
    m_vehicle->addWheel(  btVector3(3,1.2f,4),   //offset
        axis,  //direction of suspension force( 0,-1, 0)
        axle,  // Axle (-1,0,0) or (1,0,0)
        0.6f,               //arbitary rest length
        1.0f,              //arbitary radius
        tuning,             //created above
        true);
    /* Front Left */
    m_vehicle->addWheel(  btVector3(-3,1.2f,4),   //offset
        axis,  //direction of suspension force( 0,-1, 0)
        axle,  // axle (-1,0,0) or (1,0,0)
        0.6f,               //arbitary
        1.0f,               //arbitary
        tuning,             //created above
        true);
    /* back right */
    m_vehicle->addWheel(  btVector3(3,1.2f,-4.35f),   //offset
        axis,  //direction of suspension force( 0,-1, 0)
        axle,  // axle (-1,0,0) or (1,0,0)
        0.6f,               //arbitary
        1.0f,               //arbitary
        tuning,             //created above
        true);
    /* Back Left */
    m_vehicle->addWheel(  btVector3(-3,1.2f,-4.35f),   //offset
        axis,  //direction of suspension force( 0,-1, 0)
        axle,  // Axle (-1,0,0) or (1,0,0)
        0.6f,               //arbitary
        1.0f,               //arbitary
        tuning,             //created above
        true);
    for ( int i = 0; i < m_vehicle->getNumWheels(); i++)
    {
        btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
        wheel.m_rollInfluence = 1.5f;
    }
    Engine::Instance()->GetCurrentState()->GetPhysicsEngine()->GetWorld()->addVehicle(m_vehicle);
    return true;
}
void BPhysicsActor::CreateAsPlane( const btVector3& normal, float offset, const btVector3& drawingScale )
{
    m_mesh = new DirectXMesh();
    m_mesh->CreateAsPlane();
    m_mesh->SetTexture(Engine::Instance()->GetResource()->GetTexture("floor.png"));
    m_scale = drawingScale;
    m_shape = new btCompoundShape();
    btTransform t; t.setIdentity();
    m_shape->addChildShape(t, new btStaticPlaneShape(normal, offset));
    m_shape->recalculateLocalAabb();
    GenerateBody();
    m_body->setFriction(1.5f);
}
void BPhysicsActor::CreateAsCube()
{
    m_type = BPhysicsActor::Actor_Dynamic;
    //scale is managed in conscutrtor
    m_mesh = GET_MESH("cube");
    m_shape = new btCompoundShape();
    btBoxShape* shape = new btBoxShape(m_scale);
    btTransform t; t.setIdentity();
    m_shape->addChildShape(t,shape);
    m_mesh->SetTexture(GET_TEXTURE("cube.jpg"));
    m_shape->recalculateLocalAabb();
    GenerateBody();

    m_aip_Obj = new Perception::Ai_Perception_Obj(Perception::OCCLUDER,&m_d3dTransform,this);
    m_aip_Obj->CreateAsCube(m_scale.x(), m_scale.y(), m_scale.z());
    Perception::Ai_Perception_Manager::Instance()->RegisterObject(m_aip_Obj);
}
bool BPhysicsActor::CreateFromMesh( DirectXMesh* mesh ,bool staticPhysics)
{
    m_mesh = mesh;
    D3DXMATRIXA16 identity;
    D3DXMatrixIdentity(&identity);
    if(staticPhysics)
        CreateBVH(identity);
    else
        CreateConvexHull(identity);

    GenerateBody();
    return true;
}
bool BPhysicsActor::CreateFromMesh( const char* name ,bool staticPhysics)
{
    m_mesh = new DirectXMesh();
    if(m_mesh->CreateFromOBJ("Meshes/",name))
    {
        D3DXMATRIXA16 identity;
        D3DXMatrixIdentity(&identity);
        if(staticPhysics)
            CreateBVH(identity);
        else
            CreateConvexHull(identity);

        GenerateBody();
        return true;
    }
    return false;
}
void BPhysicsActor::CreateAsSphereObstacle(float radius)
{
    m_mesh = new DirectXMesh();
    m_mesh->CreateAsSphere(radius);
    m_AiCollisionRadius = radius;
    m_isStaticObstacle = true;
    m_shape = new btCompoundShape();
    btTransform t; t.setIdentity();
    m_shape->addChildShape(t,new btSphereShape(m_AiCollisionRadius));
    m_scale.setValue(1,1,1);
    m_shape->recalculateLocalAabb();
    GenerateBody();
}
void BPhysicsActor::CreateAsWallObstacle( const btVector3& from, const btVector3& to, float thickNess )
{
    m_isStaticObstacle = true;
    //get the stats of the rectangle that will make this wall
    btVector3 normalised = from-to;
    float length = (normalised).length();
    normalised.normalize();
    if(length < 1)
        return;
    float height = 10; //HARD CODED
    btVector3 halfElements(length, height, thickNess);
    //Scale is used in the physics code aswell, so it needs to stay like this
    halfElements *= 0.5f;
    m_scale = halfElements;

    //now create the mesh
    m_mesh = new DirectXMesh();
    //after we create the mesh, our scale should go back to normal
    m_mesh->CreateAsRectangle();
    m_mesh->SetTexture(Engine::Instance()->GetResource()->GetTexture("brick.jpg"));
    //now create a static cube shape
    m_shape = new btCompoundShape();
    btTransform t; t.setIdentity();
    m_shape->addChildShape(t,new btBoxShape(halfElements));
    m_shape->recalculateLocalAabb();
    //do this here, and then call Generate body to assure uniform variables
    float angle = normalised.angle(btVector3(1,0,0));
    t.setRotation(btQuaternion(btVector3(0,1,0), angle));
    t.setOrigin((from+to)*0.5f);
    m_motionState = new BPhysicsMotionState(t, m_scale);
    btVector3 inertia;
    m_shape->calculateLocalInertia(m_mass,inertia);
    btRigidBody::btRigidBodyConstructionInfo ri(m_mass,m_motionState,m_shape,inertia);
    m_body = new btRigidBody(ri);
    GenerateBody();

    m_aip_Obj = new Perception::Ai_Perception_Obj(Perception::OCCLUDER,&m_d3dTransform,this);
    m_aip_Obj->CreateAsCube(m_scale.x(), m_scale.y(), m_scale.z());
    Perception::Ai_Perception_Manager::Instance()->RegisterObject(m_aip_Obj);
}
/************************************************************************/
/* DRAWING                                                              */
/************************************************************************/
void BPhysicsActor::Render( ID3DXEffect* effect, Camera* cam)
{
    if(!m_mesh)
        return;
    if(m_body && m_body->getCollisionShape() && cam != NULL)
    {
        btVector3 pos;
        float rad = 0.0f;
        m_body->getCollisionShape()->getBoundingSphere(pos,rad);
        pos +=m_body->getWorldTransform().getOrigin();
        //draws some info about the spheres
        //for(int i = 0; i  < 10; ++i )
        //{
        //    btVector3 dir;
        //    dir.setY(0);
        //    float ang = 36.0f * i;
        //    dir.setX(cos(ang*M_PI/180.0f));
        //    dir.setZ(sin(ang*M_PI/180.0f));
        //    Engine::Instance()->drawLine(pos, pos + (dir * rad), btVector3(0,1,1));
        //}
        if(cam->GetFrustum().IsSphereInFrustum(Vec3(pos.x(),pos.y(),pos.z()), rad) == Frustum::OUTSIDE)
            return;
    }
    //get the current position and orientation
    btTransform trans;
    m_motionState->getWorldTransform(trans);
    float worldArr[16];
    trans.getOpenGLMatrix(worldArr);
    D3DXMATRIXA16 world(worldArr);

    float rotArray[16];
    m_transform.getOpenGLMatrix(rotArray);
    D3DXMATRIXA16 rot(rotArray);

    btVector3 s = m_motionState->GetGraphicsScale();
    //get the graphical scale
    D3DXMATRIXA16 scale;
    D3DXMatrixIdentity(&scale);
    D3DXMatrixScaling(&scale, s.x(), s.y(), s.z());
    world = scale*rot*world;
    //if we have a perception object, make sure it amtches our rendering orientation
    if(m_aip_Obj)
    {
        m_d3dTransform = world;
    }
    TEST( effect->SetMatrix( "g_mWorld", &world ) );
    m_mesh->Render(effect);

    if(!m_tyreMesh || !m_vehicle)
        return;

    TEST(effect->SetTexture("Diffuse", NULL))
        for(int i = 0; i < m_vehicle->getNumWheels(); ++i)
        {
            m_vehicle->updateWheelTransform(i,true);

            btWheelInfo info = m_vehicle->getWheelInfo(i);

            btTransform trans = info.m_worldTransform;

            float array[16];
            trans.getOpenGLMatrix(array);
            D3DXMATRIXA16 world(array);
            D3DXMATRIXA16 scale;
            D3DXMatrixIdentity(&scale);
            D3DXMatrixScaling(&scale, 0.8f, info.m_wheelsRadius-0.2f, info.m_wheelsRadius-0.2f);
            D3DXMatrixMultiply(&world, &scale, &world);
            TEST( effect->SetMatrix( "g_mWorld", &world) );
            m_tyreMesh->Render(effect);
        }
}
/************************************************************************/
/* PHYSICS HELPER FUNCTIONS                                             */
/************************************************************************/
BPhysicsMotionState* BPhysicsActor::GetMotionStatePointer()
{
    return m_motionState;
}
btCollisionShape* BPhysicsActor::GetShapePointer()
{
    return m_shape;
}
btRigidBody* BPhysicsActor::GetBodyPointer()
{
    return m_body;
}
const BPhysicsMotionState& BPhysicsActor::GetMotionState()const
{
    return *m_motionState;
}
void BPhysicsActor::SetActive( bool active )
{
    if(active && m_body->getActivationState() != ACTIVE_TAG)
        m_body->setActivationState(ACTIVE_TAG);
    else if(!active && m_body->getActivationState() == ACTIVE_TAG)
        m_body->setActivationState(ISLAND_SLEEPING);
}
void BPhysicsActor::SetPosition( const btVector3& pos )
{
    m_body->getWorldTransform().setOrigin(pos);
    btTransform t;
    t.setIdentity();
    t.setOrigin(pos);
    m_body->getMotionState()->setWorldTransform(t);
}
void BPhysicsActor::SteerLeft( float val )
{
    if(m_vehicle)
    {
        m_steeringValue = val;
    }
}
void BPhysicsActor::SteerCentre()
{
    if(m_vehicle)
    {
        m_steeringValue = 0;
    }
}
void BPhysicsActor::SteerRight( float val )
{
    if(m_vehicle)
    {
        m_steeringValue = -val;
    }
}
void BPhysicsActor::ApplyCentralForce( const btVector3& force )
{
    if((m_body->getActivationState() == ACTIVE_TAG ||
        m_body->getActivationState() == DISABLE_DEACTIVATION) &&
        m_isAwake == true)
    {    
        if(m_vehicle)
        {
            btVector3 nForce = force.normalized();
            float dp = m_heading.dot(nForce);
            if(m_thinking)//if we are an AI car, we need to work out where to steer!
            {
                float ang = acosf(m_rightVector.dot(nForce));
                ang *= 180 / (float)M_PI;
                if( dp < 0)
                {
                    if(ang >=90 )
                    {
                        SteerLeft(1.3f);
                    }
                    else
                        SteerRight(1.3f);

                    m_vehicle->applyEngineForce(force.length() * 1.8f,0);
                    m_vehicle->applyEngineForce(force.length() * 1.8f,1);
                    return;//>>>>>EARLY EXIT
                }
                else
                {
                    if(ang > 105)
                    {
                        SteerLeft(0.3f);
                    }
                    else if(ang < 75)
                        SteerRight(0.3f);
                    else 
                        SteerCentre();
                }
                m_vehicle->applyEngineForce(force.length(),0);
                m_vehicle->applyEngineForce(force.length(),1);
            }
            else 
            {
                if( dp < 0)
                {
                    m_vehicle->applyEngineForce(-force.length(),0);
                    m_vehicle->applyEngineForce(-force.length(),1);
                }
                else
                {
                    m_vehicle->applyEngineForce(force.length(),0);
                    m_vehicle->applyEngineForce(force.length(),1);
                }
            }
        }
        else
            m_body->applyCentralForce(force);
    }
}
void BPhysicsActor::SetFriction( float fric )
{
    m_body->setFriction(fric);
}
void BPhysicsActor::ApplyForceAtPoint( const btVector3& force,const btVector3& point )
{
    if(m_body->getActivationState() == ACTIVE_TAG ||
        m_body->getActivationState() == DISABLE_DEACTIVATION)

        //

        m_body->applyForce(force, point);
}
DirectXMesh* BPhysicsActor::GetMesh()
{
    return m_mesh;
}
void BPhysicsActor::ApplyBrakes( float param1 )
{
    m_breakingForce = param1 * m_maxBrakes;
}
/************************************************************************/
/* AI HELPER FUNCTIONS                                                  */
/************************************************************************/
void BPhysicsActor::HandleInputs()
{
    if(m_controlls[BRAKE])
        ApplyBrakes((1.0f));
    else
        ApplyBrakes(0.0f);

    if(m_controlls[FORWARD])
        ApplyCentralForce(m_heading * 500);
    else if(m_controlls[BACKWARD])
        ApplyCentralForce(m_heading * -500);
    else
        ApplyCentralForce(GetLinearVelocity()*0.05f);

    if(m_controlls[LEFT])
        SteerLeft(0.03f);
    else if(m_controlls[RIGHT])
        SteerRight(0.03f);
    else
        SteerCentre();
}
void BPhysicsActor::Update(float dt)
{
    btTransform t = m_body->getWorldTransform();
    if(m_body)
    {
        btMatrix3x3 m = t.getBasis();
        m_heading = m.getColumn(0);
        m_rightVector  = m.getColumn(2);
    }
    if(m_vehicle)
    {
        if(!m_thinking) //if we are not an AI :)
            HandleInputs();

        if(m_engineNode && m_engineNode->GetSourceNotifier() <= 0)
            m_engineNode->SetPosition(Vec3(t.getOrigin().x(),
                                           t.getOrigin().y(),
                                           t.getOrigin().z()));
        m_heading = m_vehicle->getChassisWorldTransform().getBasis().getColumn(2);
        m_rightVector = m_vehicle->getChassisWorldTransform().getBasis().getColumn(0);
        //if we are steering neurtal
        if(m_steeringValue == 0)
        {
            //Front Left Wheel
            if(m_vehicle->getSteeringValue(0) < -0.1f &&
                m_vehicle->getSteeringValue(0) > -1.0f)
            {
                m_vehicle->setSteeringValue(m_vehicle->getSteeringValue(0)+m_steerResetSpeed,0);
            }
            else if (m_vehicle->getSteeringValue(0) > 0.1f &&
                m_vehicle->getSteeringValue(0) < 1.0f)
            {
                m_vehicle->setSteeringValue(m_vehicle->getSteeringValue(0)-m_steerResetSpeed,0);
            }
            else
                m_vehicle->setSteeringValue(0,0);

            //Front Right wheel
            if(m_vehicle->getSteeringValue(1) < -0.1f &&
                m_vehicle->getSteeringValue(1) > -1.0f)
            {
                m_vehicle->setSteeringValue(m_vehicle->getSteeringValue(1)+m_steerResetSpeed,1);
            }
            else if (m_vehicle->getSteeringValue(1) > 0.1f &&
                m_vehicle->getSteeringValue(1) < 1.0f)
            {
                m_vehicle->setSteeringValue(m_vehicle->getSteeringValue(1)-m_steerResetSpeed,1);
            }
            else
                m_vehicle->setSteeringValue(0,1);
        }
        else
        {
            if(m_vehicle->getSteeringValue(0) > -0.65f && m_steeringValue > 0)
                m_vehicle->setSteeringValue(m_vehicle->getSteeringValue(0)-m_steeringValue,0);
            if(m_vehicle->getSteeringValue(1) > -0.65f && m_steeringValue > 0)
                m_vehicle->setSteeringValue(m_vehicle->getSteeringValue(1)-m_steeringValue,1);
            //steering value is negative, so minusing it is Adding it :)
            if(m_vehicle->getSteeringValue(0) < 0.65f && m_steeringValue < 0)
                m_vehicle->setSteeringValue(m_vehicle->getSteeringValue(0)-m_steeringValue,0);
            if(m_vehicle->getSteeringValue(1) < 0.65f && m_steeringValue < 0)
                m_vehicle->setSteeringValue(m_vehicle->getSteeringValue(1)-m_steeringValue,1);
        }

        m_vehicle->setBrake(m_breakingForce, 2);
        m_vehicle->setBrake(m_breakingForce, 3);
    }

    if(m_thinking)
    {
        Think(dt);
    }

    if(m_aip_Obj)
    {
        //get the current position and orientation
        btTransform trans;
        m_motionState->getWorldTransform(trans);
        float worldArr[16];
        trans.getOpenGLMatrix(worldArr);
        D3DXMATRIXA16 world(worldArr);

        float rotArray[16];
        m_transform.getOpenGLMatrix(rotArray);
        D3DXMATRIXA16 rot(rotArray);

        btVector3 s = m_motionState->GetGraphicsScale();
        //get the graphical scale
        D3DXMATRIXA16 scale;
        D3DXMatrixIdentity(&scale);
        D3DXMatrixScaling(&scale, s.x(), s.y(), s.z());
        world = scale*rot*world;
        m_d3dTransform = world;
    }
}
void BPhysicsActor::Think(float dt)
{
    if(!m_perceiver->HasRequest() /* && NeedsRequest() */)
    {    
        btTransform t = m_body->getWorldTransform();
        D3DXVECTOR3 position(t.getOrigin().x(),t.getOrigin().y(),t.getOrigin().z());
        D3DXVECTOR3 direction(m_heading.x(),m_heading.y(),m_heading.z());
        D3DXVECTOR3 up(0,1,0);
        m_perceiver->SetViewMatrix(position,direction,up);
        m_perceiver->NewRequest();
    }
    if(m_perceiver->TextureReady())
    {
        ProcessPerceptionBuffer();

    }
    m_manager.Update();
}
void BPhysicsActor::ProcessPerceptionBuffer()
{
    Perception::PixelColor8* buffer = m_perceiver->GetBuffer();
    int count = 0;
    for (int i = 0; i < m_resX*m_resY; ++i)
    {
        count += buffer[i].r;
        int y = (i / m_resX);
        int x = (i % m_resX);
        if(buffer[i].r > 10)
        {
            D3DXVECTOR3 dir = m_perceiver->GetDirection(x,y);
            btVector3 pos (m_body->getWorldTransform().getOrigin());
            btVector3 btDir(dir.x,dir.y,dir.z);
            Engine::Instance()->GetLineDrawer()->drawLine(pos, pos+(btDir*50), btVector3(1,1,0));
        }
    }
    //so 0 is no red, 255 is full red, we can work out a % with that
    //float total = (float)count / (float)(m_resX*m_resY);
    //float percent = (total / 255.0f)*100;
    //std::ostringstream oss;
    //oss << percent << "% ";
    //std::string s = oss.str();
    //Engine::Instance()->GetCurrentState()->HandleMessage(s.c_str());
}
void BPhysicsActor::CreatePerceptionOccluder(Perception::Ai_Perception_Key key, DirectXMesh* mesh)
{
    m_aip_Obj = new Perception::Ai_Perception_Obj(key,&m_d3dTransform,this);
    m_aip_Obj->CreateFromMesh(mesh, 1,1,1);
    Perception::Ai_Perception_Manager::Instance()->RegisterObject(m_aip_Obj);
}
btVector3 BPhysicsActor::GetPosition() const
{
    btTransform t;
    m_motionState->getWorldTransform(t);
    return t.getOrigin();
}
const btVector3& BPhysicsActor::GetLinearVelocity() const
{
    return m_body->getLinearVelocity();
}
bool BPhysicsActor::HasTarget()
{
    if(m_target)
        return true;
    return false;
}
BPhysicsActor* BPhysicsActor::GetTarget()
{
    return m_target;
}
Grid* BPhysicsActor::GetGrid()
{
    return m_grid;
}
void BPhysicsActor::CreateAi( BPhysicsActor* target, float aiRadius, 
    float maxVel, bool startThinking , const char* luaObj)
{
    SetTarget(target);
    SetAiCollisionRadius(aiRadius);
    SetMaxVelocity(maxVel);
    SetThinking(startThinking);
    InitializeStateManager(luaObj);
}
void BPhysicsActor::CreatePerspProjection(float fOv, float aspect, float n, float f)
{
    m_projectionType = 0;
    m_perceiver->SetProjectionMatrixPerspective(fOv, aspect, n, f);
}
void BPhysicsActor::CreateOrthoProjection(int width, int height, float n, float f)
{
    m_projectionType = 1;   
    m_perceiver->SetProjectionMatrixOrthographic((float)width,(float)height,n,f);
}
void BPhysicsActor::SetUpPerception(int x, int y)
{
    m_perceiver = new Perception::Ai_Perceiver(this);
    m_perceiver->Initialize(x,y);
    m_resX = x; m_resY = y;
}
/* This call back is called if a collision is detected between two objects
passed into the dynamicsworld->ContactPairTest.

In this instance, we want to remove the node 'm_index' from the graph 'm_g'
and do nothing else with the collision */
struct MyResultCallBack : public btCollisionWorld::ContactResultCallback
{
    Grid* m_g;
    int m_index;
    virtual	btScalar addSingleResult(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
    {
        m_g->GraphPtr()->RemoveNode(m_index);
        return 0.0f;
    }
};
void BPhysicsActor::UpdateGrid() const
{
    if(m_isStaticObstacle)
    {
        if(m_body->getCollisionShape()->getShapeType() ==  SPHERE_SHAPE_PROXYTYPE)
        {
            btVector3 p = m_body->getWorldTransform().getOrigin();
            D3DXVECTOR3 pos(p.x(), p.y(), p.z());
            float radSqr = m_AiCollisionRadius*2*m_AiCollisionRadius*2;

            for(unsigned int i = 0; i<m_grid->GetGraph().Nodes().size(); ++i)
            {
                if(m_grid->GetGraph().Nodes()[i].GetID() == -1)
                    continue;
                D3DXVECTOR3 toNode = pos - m_grid->GetGraph().Nodes()[i].GetPos();
                float dist = D3DXVec3LengthSq(&toNode);
                if( dist < radSqr)
                {
                    m_grid->GraphPtr()->RemoveNode(i);
                }
            }
        }
        else if(m_body->getCollisionShape()->getShapeType() == BOX_SHAPE_PROXYTYPE)
        {
            btVector3 p;
            btScalar rad;
            m_body->getCollisionShape()->getBoundingSphere(p,rad);
            p = m_body->getWorldTransform().getOrigin();
            D3DXVECTOR3 pos(p.x(), p.y(), p.z());
            float radSqr = rad*rad;
            //crappy
            BPhysicsActor* actr = new BPhysicsActor(0.0f, btVector3(1,1,1), m_grid);
            actr->CreateAsSphereObstacle(5);
            MyResultCallBack cb;
            //for each node...
            for(unsigned int i = 0; i<m_grid->GetGraph().Nodes().size(); ++i)
            {
                if(m_grid->GetGraph().Nodes()[i].GetID() == -1)
                    continue;
                //quick sphere check
                D3DXVECTOR3 toNode = pos - m_grid->GetGraph().Nodes()[i].GetPos();
                float dist = D3DXVec3LengthSq(&toNode);
                if(dist > radSqr)
                    continue;
                //set the position
                btVector3 pos(m_grid->GetGraph().Nodes()[i].GetPos().x, 0, m_grid->GetGraph().Nodes()[i].GetPos().z);
                actr->SetPosition(pos);
                //set up the callback
                cb.m_g = m_grid;
                cb.m_index = i;
                //Quickly test for collision, and call the addSingleResult function.
                Engine::Instance()->GetCurrentState()->GetPhysicsEngine()->GetWorld()->contactPairTest(m_body, actr->GetBodyPointer(), cb );
            }
            actr->CleanUp();
            delete actr->GetMotionStatePointer();
            delete actr->GetShapePointer();
            delete actr->GetBodyPointer();
            delete actr;
        }
    }
}
/************************************************************************/
/* SHUT DOWN                                                            */
/************************************************************************/
void BPhysicsActor::CleanUp()
{
    if(m_vehicle)
    {
        if(m_wavPlayerNode)
            m_wavPlayerNode->m_active = false; //this will mark the engine node for deletion too
        if(m_engineNode)
            m_engineNode = NULL;
        Engine::Instance()->GetCurrentState()->GetPhysicsEngine()->RemoveVehicle(m_vehicle);
        delete m_vehicle;
        m_vehicle = 0;
        if(m_rayCaster)
            delete m_rayCaster;
        m_rayCaster = 0;
    }

    if(m_mesh && !m_mesh->IsShared())
    {
        m_mesh->CleanUp();
        delete m_mesh;
        m_mesh = 0;
    }
    if(m_meshIndexs)
    {
        delete[] m_meshIndexs;
        m_meshIndexs = 0;
    }
    if(m_meshVertexs)
    {
        delete[] m_meshVertexs;
        m_meshVertexs = 0;
    }
    if(m_array)
    {
        delete m_array;
        m_array = 0;
    }
    if(m_triangleMesh)
    {
        delete m_triangleMesh;
        m_triangleMesh = 0;
    }
    if(m_aip_Obj)
    {
        delete m_aip_Obj;
        m_aip_Obj = 0;
    }
    if(m_perceiver)
    {
        m_perceiver->CleanUp();
        delete m_perceiver;
        m_perceiver = 0;
    }
}
